use std::{sync::Arc, thread::sleep, time::Duration};

use axum::{response::IntoResponse, routing::get, Extension, Json, Router};
use realman::traits::{ModbusTrait, MotionTrait};
use serde::{Deserialize, Serialize};

use crate::{
    server::{ApiOk, AppState, ArmCmd},
    Result,
};

pub fn router() -> Router {
    Router::new().route("/", get(index).post(control_arm))
}

// 获取机械臂当前的状态
async fn index(Extension(app_state): Extension<Arc<AppState>>) -> impl IntoResponse {
    #[derive(Debug, Serialize, Default)]
    struct Joint {
        joint: Vec<i32>,
    }

    let mut data = vec![0; 7];
    if let Ok(joint) = app_state.arm_joint.lock() {
        data = joint.to_vec();
    }

    Json(Joint { joint: data })
}

#[derive(Debug, Deserialize)]
struct PostData {
    cmd: ArmCmd,
}
// 控制机械臂
async fn control_arm(
    Extension(app_state): Extension<Arc<AppState>>,
    body: String,
) -> Result<impl IntoResponse> {
    let post_data = serde_json::from_str::<PostData>(&body)?;

    let speed = 10;
    if let Ok(mut arm) = app_state.arm_listen.lock() {
        match post_data.cmd {
            ArmCmd::Stop => {
                arm.set_arm_slow_stop().ok();
            }
            ArmCmd::Froward => {
                arm.set_pos_step(realman::StepType::XStep, 20000, speed)
                    .ok();
            }
            ArmCmd::Back => {
                arm.set_pos_step(realman::StepType::XStep, -20000, speed)
                    .ok();
            }
            ArmCmd::Left => {
                arm.set_pos_step(realman::StepType::YStep, 20000, speed)
                    .ok();
            }
            ArmCmd::Right => {
                arm.set_pos_step(realman::StepType::YStep, -20000, speed)
                    .ok();
            }
            ArmCmd::Up => {
                arm.set_pos_step(realman::StepType::ZStep, 20000, speed)
                    .ok();
            }
            ArmCmd::Down => {
                arm.set_pos_step(realman::StepType::ZStep, -20000, speed)
                    .ok();
            }
            ArmCmd::TurnLeft => {
                arm.set_ort_step(realman::RStepType::RZStep, -500, speed)
                    .ok();
            }
            ArmCmd::TureRight => {
                arm.set_ort_step(realman::RStepType::RZStep, 500, speed)
                    .ok();
            }
            ArmCmd::Open => {
                arm.write_single_register(realman::ModbusPortType::EndRS485RTUMain, 40000, 100, 1)
                    .ok();
                sleep(Duration::from_secs(1));
            }
            ArmCmd::Close => {
                arm.write_single_register(realman::ModbusPortType::EndRS485RTUMain, 40000, 0, 1)
                    .ok();
                sleep(Duration::from_secs(1));
            }
            ArmCmd::ZLeft => {
                arm.set_ort_step(realman::RStepType::RXStep, -50, speed)
                    .ok();
            }
            ArmCmd::ZRight => {
                arm.set_ort_step(realman::RStepType::RXStep, 50, speed).ok();
            }
            ArmCmd::Init => {
                arm.movej(
                    realman::ArmType::Gen72([0, 50000, 0, -54000, 0, 18000, 725]),
                    speed,
                    0,
                    realman::TrajectoryConnect::Now,
                )
                .ok();
            }
            ArmCmd::Home => {
                arm.movej(
                    realman::ArmType::Gen72([0; 7]),
                    speed,
                    0,
                    realman::TrajectoryConnect::Now,
                )
                .ok();
            }
        }
    }

    Ok(Json(ApiOk::OK))
}
